/*
    Copyright 2021 codenocold codenocold@qq.com
    Address : https://github.com/codenocold/dgm
    This file is part of the dgm firmware.
    The dgm firmware is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    The dgm firmware is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "usr_config.h"
#include "controller.h"
#include "util.h"
#include <string.h>

tUsrConfig UsrConfig;
tCoggingMap *pCoggingMap = NULL;

void USR_CONFIG_set_default_config(void)
{
    // Motor
    UsrConfig.invert_motor_dir = 0;
    UsrConfig.inertia = 0.0001f;
    UsrConfig.torque_constant = 0.051274f;
//    UsrConfig.torque_constant = 1;
    UsrConfig.motor_pole_pairs = 7;
    UsrConfig.motor_phase_resistance = 0.11247408f;
    UsrConfig.motor_phase_inductance = 0.00002235f;
    UsrConfig.current_limit = 8;
    UsrConfig.velocity_limit = 60;

    // Encoder
    UsrConfig.calib_current = 5.0f;
    UsrConfig.calib_voltage = 3.0f;

    // Anticogging
    UsrConfig.anticogging_enable = 0;

    // Controller
    UsrConfig.control_mode = CONTROL_MODE_VELOCITY_RAMP;
    UsrConfig.pos_gain = 80.0f;
    UsrConfig.vel_gain = 0.03f;
    UsrConfig.vel_integrator_gain = 0.01f;
    UsrConfig.current_ctrl_bw = 2000;
    UsrConfig.sync_target_enable = 0;
    UsrConfig.target_velcity_window = 0.5f;
    UsrConfig.target_position_window = 0.01f;
    UsrConfig.torque_ramp_rate = 0.1f;
    UsrConfig.velocity_ramp_rate = 100.0f;
    UsrConfig.position_filter_bw = 2;
    UsrConfig.profile_velocity = 30;
    UsrConfig.profile_accel = 50;
    UsrConfig.profile_decel = 50;

    // Protect
    UsrConfig.protect_under_voltage = 10;
    UsrConfig.protect_over_voltage = 14;
    UsrConfig.protect_over_current = 10;
    UsrConfig.protect_i_bus_max = 3.0f;

    // CAN
    UsrConfig.node_id = 1;
    UsrConfig.can_baudrate = CAN_BAUDRATE_500K;
    UsrConfig.heartbeat_consumer_ms = 0;
    UsrConfig.heartbeat_producer_ms = 0;

    // Encoder
    UsrConfig.calib_valid = 0;
}


int USR_CONFIG_erease_config(void)
{
    HAL_StatusTypeDef ret = HAL_OK;

    // Erase
    ret = HAL_FLASH_Unlock();
    if (ret != HAL_OK)
        return -1;


    FLASH_EraseInitTypeDef erase_struct = {
            .TypeErase = FLASH_TYPEERASE_PAGES,
            .Banks = FLASH_BANK_1,
            .Page = USR_CONFIG_PAGE,
            .NbPages = USR_CONFIG_PAGE_SIZE,
    };
    HAL_FLASH_Unlock();
    uint32_t sector_error;
    if (HAL_FLASHEx_Erase(&erase_struct, &sector_error) != HAL_OK)
    {
        HAL_FLASH_Lock();
        return -2;
    }

//    for (uint32_t page = USR_CONFIG_PAGE; page < (USR_CONFIG_PAGE + USR_CONFIG_PAGE_SIZE); page++)
//    {
//        FLASH_EraseInitTypeDef erase_struct = {
//                .TypeErase = FLASH_TYPEERASE_PAGES,
//                .Banks = FLASH_BANK_1,
//                .Page = page,
//                .NbPages = 1,
//        };
//        HAL_FLASH_Unlock();
//        uint32_t sector_error;
//        if (HAL_FLASHEx_Erase(&erase_struct, &sector_error) != HAL_OK)
//        {
//            HAL_FLASH_Lock();
//            return -2;
//        }
//    }

    ret = HAL_FLASH_Lock();
    if (ret != HAL_OK)
        return -3;

    // Check
    for (uint32_t addr = USR_CONFIG_ADDR; addr < (USR_CONFIG_ADDR + USR_CONFIG_MAX_SIZE); addr += 4)
    {
        if (0xFFFFFFFF != *((uint32_t *) addr))
            return -4;
    }

    return (int)HAL_FLASH_GetError();
}

int USR_CONFIG_read_config(void)
{
    int state = 0;

    memcpy(&UsrConfig, (uint8_t *) USR_CONFIG_ADDR, sizeof(tUsrConfig));

    uint32_t crc = crc32((uint8_t *) &UsrConfig, sizeof(tUsrConfig) - 4);
    if (crc != UsrConfig.crc)
    {
        state = -1;
    }

    return state;
}

int USR_CONFIG_save_config(void)
{
    // Erase
    if (USR_CONFIG_erease_config())
        return -1;

    // Program
    HAL_StatusTypeDef ret = 0;

    ret = HAL_FLASH_Unlock();
    if (ret != HAL_OK)
        return -2;

    UsrConfig.crc = crc32((uint8_t *) &UsrConfig, sizeof(tUsrConfig) - 4);
    uint32_t *pData = (uint32_t *) &UsrConfig;

    for (int i = 0; i < sizeof(tUsrConfig) / 8 + 1; i++)
    {
        uint64_t dataL = *(pData + 2 * i);
        uint64_t dataH = *(pData + 2 * i + 1);
        uint64_t data = dataH << 32 | dataL;

        //__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);
        ret = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, USR_CONFIG_ADDR + 8 * i, data);
        printf("USR_CONFIG_save_config HAL_FLASH_Program ret = %d\n", ret);
        if (ret != HAL_OK)
        {
            HAL_FLASH_Lock();
            return -3;
        }
    }
    ret = HAL_FLASH_Lock();
    if (ret != HAL_OK)
        return -4;

    return 0;
}

void USR_CONFIG_set_default_cogging_map(void)
{
    if (pCoggingMap == NULL)
    {
        pCoggingMap = malloc(sizeof(tCoggingMap));
        if (pCoggingMap == NULL)
        {
            printf("USR_CONFIG_set_default_cogging_map faild, malloc error\n");
        }
    }

    for (int i = 0; i < COGGING_MAP_NUM; i++)
    {
        pCoggingMap->map[i] = 0;
        memset(pCoggingMap->map, 0, sizeof(pCoggingMap->map[0] * COGGING_MAP_NUM));
    }
}

int USR_CONFIG_erease_cogging_map(void)
{
    HAL_StatusTypeDef ret = 0;

    // Erase
    ret = HAL_FLASH_Unlock();
    if (ret != HAL_OK)
        return -1;

    for (uint32_t page = COGGING_MAP_PAGE; page < (COGGING_MAP_PAGE + COGGING_MAP_PAGE_SIZE); page++)
    {
        FLASH_EraseInitTypeDef erase_struct = {
                .TypeErase = FLASH_TYPEERASE_PAGES,
                .Banks = FLASH_BANK_1,
                .Page = page,
                .NbPages = 1,
        };
        HAL_FLASH_Unlock();
        uint32_t sector_error;
        if (HAL_FLASHEx_Erase(&erase_struct, &sector_error) != HAL_OK)
        {
            HAL_FLASH_Lock();
            return -2;
        }
    }

    ret = HAL_FLASH_Lock();
    if (ret != HAL_OK)
        return -3;

    // Check
    for (uint32_t addr = COGGING_MAP_ADDR; addr < (COGGING_MAP_ADDR + COGGING_MAP_MAX_SIZE); addr += 4)
    {
        if (0xFFFFFFFF != *((uint32_t *) addr))
        {
            return -4;
        }
    }

    return 0;
}

int USR_CONFIG_read_cogging_map(void)
{
    int state = 0;

    if (pCoggingMap == NULL)
    {
        pCoggingMap = malloc(sizeof(tCoggingMap));
        if (pCoggingMap == NULL)
        {
            printf("USR_CONFIG_read_cogging_map: malloc err\n");
        }
    }

    memcpy(pCoggingMap, (uint8_t *) COGGING_MAP_ADDR, sizeof(tCoggingMap));

    uint32_t crc;
    crc = crc32((uint8_t *) pCoggingMap, sizeof(tCoggingMap) - 4);
    if (crc != pCoggingMap->crc)
    {
        state = -1;
    }

    return state;
}

int USR_CONFIG_save_cogging_map(void)
{
    // Erase
    if (USR_CONFIG_erease_cogging_map())
    {
        return -1;
    }
    printf("USR_CONFIG_save_cogging_map: erase ok\n");

    // Program
    HAL_StatusTypeDef ret = 0;

    ret = HAL_FLASH_Unlock();
    if (ret != HAL_OK)
        return -2;

    pCoggingMap->crc = crc32((uint8_t *) pCoggingMap, sizeof(tCoggingMap) - 4);
    uint32_t *pData = (uint32_t *) pCoggingMap;
    for (int i = 0; i < sizeof(tCoggingMap) / 8 + 1; i++)
    {
        uint64_t dataL = *(pData + 2 * i);
        uint64_t dataH = *(pData + 2 * i + 1);
        uint64_t data = dataH << 32 | dataL;

        //__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);
        printf("USR_CONFIG_save_cogging_map: HAL_FLASH_Program ret = %d\n", ret);
        ret = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, COGGING_MAP_ADDR + i * 8, data);
        if (ret != HAL_OK)
        {
            HAL_FLASH_Lock();
            return -2;
        }
    }
    ret = HAL_FLASH_Lock();
    if (ret != HAL_OK)
        return -4;

    printf("USR_CONFIG_save_cogging_map ok\n");
    return 0;
}
